#pragma once
#include "BasicThetaStar.h"

class LazyThetaStar :public BasicThetaStar
{
public:

	LazyThetaStar(GridGraph graph, int sx, int sy, int ex, int ey)
	:BasicThetaStar(graph, sx, sy, ex, ey)
	{

	}

	void computePath() {
			int totalSize = (graph.sizeX + 1) * (graph.sizeY + 1);

			int start = toOneDimIndex(sx, sy);
			finish = toOneDimIndex(ex, ey);

			pq =ReusableIndirectHeap(totalSize);
			initialiseMemory(totalSize, FLT_MAX, -1, false);

			initialise(start);

			while (!pq.isEmpty()) {
				int current = pq.popMinIndex();
				int x = toTwoDimX(current);
				int y = toTwoDimY(current);

				int parentIndex = parent(current);
				if (parentIndex != -1) {
					int parX = toTwoDimX(parentIndex);
					int parY = toTwoDimY(parentIndex);

					if (!graph.lineOfSight(x, y, parX, parY)) {
						findPath1Parent(current, x, y);
					}
				}

				if (current == finish || distance(current) == FLT_MAX) {
					//maybeSaveSearchSnapshot();
					break;
				}
				setVisited(current, true);

				tryRelaxNeighbour(current, x, y, x - 1, y - 1);
				tryRelaxNeighbour(current, x, y, x, y - 1);
				tryRelaxNeighbour(current, x, y, x + 1, y - 1);

				tryRelaxNeighbour(current, x, y, x - 1, y);
				tryRelaxNeighbour(current, x, y, x + 1, y);

				tryRelaxNeighbour(current, x, y, x - 1, y + 1);
				tryRelaxNeighbour(current, x, y, x, y + 1);
				tryRelaxNeighbour(current, x, y, x + 1, y + 1);

				//maybeSaveSearchSnapshot();
			}

			maybePostSmooth();
		}

	void findPath1Parent(int current, int x, int y) {
		setDistance(current, FLT_MAX);
		for (int i = -1; i <= 1; ++i) {
			for (int j = -1; j <= 1; ++j) {
				if (i == 0 && j == 0) continue;
				int px = x + i;
				int py = y + j;
				if (!graph.isValidBlock(px, py)) continue;
				int index = graph.toOneDimIndex(px, py);
				if (!visited(index)) continue;
				if (!graph.neighbourLineOfSight(x, y, px, py)) continue;

				float gValue = distance(index) + graph.distance(x, y, px, py);
				if (gValue < distance(current)) {
					setDistance(current, gValue);
					setParent(current, index);
				}
			}
		}
	}

	bool relax(int u, int v, float weightUV) {
			// return true iff relaxation is done.
			if (parent(u) != -1) {
				u = parent(u);
			}

			float newWeight = distance(u) + physicalDistance(u, v);
			if (newWeight < distance(v)) {
				setDistance(v, newWeight);
				setParent(v, u);
				return true;
			}
			return false;
		}
};
